#include "read_data.h"

void read_result(double& init_bias, std::vector<double>& timestamps, std::vector<Eigen::Vector3d>& imu_data, std::vector<Eigen::Vector3d>& odometry_data, std::vector<Eigen::Vector2d> &vel_data, std::vector<Eigen::Vector3d>& pose_data) {
	std::ifstream reader("../result.txt");
	std::string line;

	while (std::getline(reader, line)) {  
        std::istringstream iss(line);
        double timestamp;
      	Eigen::Vector3d imu;
      	Eigen::Vector3d odometry;
        Eigen::Vector2d velocity; // in world 
      	Eigen::Vector3d pose;

        if (iss >> timestamp >> init_bias >> imu[0] >> imu[1] >>  imu[2] >> odometry[0] >> odometry[1] >> odometry[2] >> pose[0] >> pose[1] >> velocity[0] >> velocity[1] >> pose[2]) {  
            // std::cout << "read: " << timestamp << ", " << init_bias << ", " << imu.transpose() << ", " << odometry.transpose() << ", " << pose.transpose() << std::endl;
        	timestamps.push_back(timestamp);
            imu_data.push_back(imu);

        	odometry_data.push_back(odometry);
            vel_data.push_back(velocity);
        	pose_data.push_back(pose);
        } else {
            std::cerr << "invalid line: " << line << std::endl;
        }
    }

	reader.close();
}


// 分割字符串为 vector<string>
vector<string> split(const string& line, char delimiter = ',') {
    vector<string> tokens;
    stringstream ss(line);
    string token;
    while (getline(ss, token, delimiter)) {
        tokens.push_back(token);
    }
    return tokens;
}

// 将一行字符串转换为 vector<double>，跳过无效值
vector<double> convert_to_double(const vector<string>& tokens) {
    vector<double> values;
    for (const auto& token : tokens) {
        try {
            // 尝试转换为 double
            double value = stod(token);
            values.push_back(value);
        } catch (const invalid_argument& e) {
            cerr << "无效数值: '" << token << "'，跳过此字段" << endl;
        } catch (const out_of_range& e) {
            cerr << "数值超出范围: '" << token << "'，跳过此字段" << endl;
        }
    }
    return values;
}

void read_sim_timestamp(std::vector<double>& timestamps) {
    std::ifstream reader("../../sim_data/meta/timestamps.txt");
    std::string line;
    
    while (std::getline(reader, line)) {  
        std::istringstream iss(line);
        double timestamp;
        if (iss >> timestamp) {  
            timestamps.push_back(timestamp);
        }
    }
}
void read_sim_gt(std::vector<Eigen::Vector3d>& pose_data) {
    std::ifstream reader("../../sim_data/meta/trajectory_gt.txt");
    std::string line;
    
    while (std::getline(reader, line)) { 
        if(line[0] == '#') {
            continue;
        } 
        std::istringstream iss(line);
        Eigen::Vector3d pose;
        if (iss >> pose[0] >> pose[1] >> pose[2]) {  
            pose_data.push_back(pose);
            // std::cout << "pose: " << pose.transpose() << std::endl;
        }
    }
}

void read_sim_imu(std::vector<Eigen::Vector3d>& imu_data) {
    std::ifstream reader("../../sim_data/imu/imu_data.csv");
    std::string line;
    
    while (std::getline(reader, line)) {  
        if(line[0] == '#') {
            continue;
        } 

        vector<string> tokens = split(line);
        vector<double> imu_vector = convert_to_double(tokens);
        // std::cout << "tokens: " << tokens.size() << std::endl;
        // std::cout << "imu_vector: " << imu_vector.size() << std::endl;
        Eigen::Vector3d imu;
        imu[0] = imu_vector[1];
        imu[1] = imu_vector[2];
        imu[2] = imu_vector[6];
        imu_data.push_back(imu);
        
        // std::cout << "imu: " << imu.transpose() << std::endl;
    }
}

void read_sim_wheel_odometry(std::vector<Eigen::Vector3d>& odometry_data) {
    std::ifstream reader("../../sim_data/odom/wheel_odom_integrated.txt");
    std::string line;
    
    while (std::getline(reader, line)) { 
        if(line[0] == '#') {
            continue;
        } 
        std::istringstream iss(line);

        // std::cout << "line ======== : " << line << std::endl;

        Eigen::Vector3d odometry;
        if (iss >> odometry[0] >> odometry[1] >> odometry[2]) {  
            odometry_data.push_back(odometry);
            // std::cout << "odometry: " << odometry.transpose() << std::endl;
        }
    }
}

void read_sim_wheel_velocity(std::vector<Eigen::Vector3d>& velocity_data) {
    std::ifstream reader("../../sim_data/odom/wheel_odom_raw.csv");
    std::string line;
    
    while (std::getline(reader, line)) { 
        if(line[0] == '#') {
            continue;
        } 

        vector<string> tokens = split(line);
        vector<double> vel_vector = convert_to_double(tokens);

        // std::cout << "line ======== : " << line << std::endl;

        Eigen::Vector3d velocity;
        velocity[0] = vel_vector[0]; // vx
        velocity[1] = vel_vector[1]; // vy
        velocity[2] = vel_vector[2]; // w
        velocity_data.push_back(velocity);
        // std::cout << "velocity: " << velocity.transpose() << std::endl;
    }
}

void read_sim_gps(std::vector<Eigen::Vector3d>& gps_data) {
    std::ifstream reader("../../sim_data/gps/gps_data.csv");
    std::string line;
    while (std::getline(reader, line)) { 
        if(line[0] == '#') {
            continue;
        } 

        vector<string> tokens = split(line);
        vector<double> gps_vector = convert_to_double(tokens);

        // std::cout << "line ======== : " << line << std::endl;
        // std::cout << "gps_vector ======== : " << gps_vector.size() << std::endl;

        Eigen::Vector3d gps;
        gps[0] = gps_vector[0];
        gps[1] = gps_vector[1];
        gps[2] = gps_vector[2];
        gps_data.push_back(gps);
        // std::cout << "gps: " << gps.transpose() << std::endl;
    }
}

void read_simulate(double& init_bias, std::vector<double>& timestamps, std::vector<Eigen::Vector3d>& imu_data,  std::vector<Eigen::Vector3d>& odometry_data, std::vector<Eigen::Vector3d> &vel_data, std::vector<Eigen::Vector3d>& pose_data, std::vector<Eigen::Vector3d>& gps_data) {
    // gyro, acc_bias = 0.0
    init_bias = 0.005;

    read_sim_timestamp(timestamps);
    read_sim_gt(pose_data);
    read_sim_imu(imu_data);
    read_sim_wheel_odometry(odometry_data);
    read_sim_wheel_velocity(vel_data);
    read_sim_gps(gps_data);
}

Eigen::Vector2d compute_velocity(const Eigen::Vector3d& last_pose, const Eigen::Vector3d& last_odom, const Eigen::Vector3d& curr_odom, const double &dt) {
    Eigen::Matrix2d rotation = math_utils::rotation(last_pose[2]);
    Eigen::Vector3d delta_vel = (curr_odom - last_odom)/dt;
    std::cout << "curr_odom ============= : " << curr_odom.transpose() << std::endl;
    std::cout << "last_odom ============= : " << last_odom.transpose() << std::endl;

    std::cout << "delta_vel ============= : " << delta_vel.transpose() << std::endl;
    std::cout << "rotation ============= : " << rotation << std::endl;

    Eigen::Vector2d vel(delta_vel[0], delta_vel[1]);

    return rotation*vel;
}


std::deque<double> convert_deque(const std::vector<double>& timestamps) {
    std::deque<double> timestamp_queue;
    for(const auto& time : timestamps) {
        timestamp_queue.push_back(time);
    }

    return timestamp_queue;
}